//
// Created by 24087 on 2025/5/13.
//

#include "Queue2.h"
#include "Interface_Motor.h"
//
// Created by 24087 on 2025/5/11.
//


const uint8_t MAX_SIZE2 = 8;

void Queue2Init(Queue2 *queue)
{
	queue->data = calloc(MAX_SIZE2, sizeof(QE_DataType2));
	queue->head = 0;
	queue->tail = 0;
	queue->size = 0;
	queue->capacity = MAX_SIZE2;
}

void Queue2Destroy(Queue2 *queue)
{
	if (queue->data)
	{
		free(queue->data);
		queue->data = NULL;
	}
	queue->head = 0;
	queue->tail = 0;
	queue->size = 0;
	queue->capacity = 0;
}

void Queue2Push(Queue2 *queue, const QE_DataType2 data)
{
	if (Queue2Full(queue))
	{
		printf("Queue2 is full\n");
		return;
	}
	queue->data[queue->tail] = data;
	queue->tail = (queue->tail + 1) % queue->capacity;
	queue->size++;
}

bool Queue2Full(const Queue2 *queue)
{
	if (queue->size == queue->capacity)
	{
		return true;
	}
	return false;
}

QE_DataType2 Queue2Pop(Queue2 *queue)
{
	if (Queue2Empty(queue))
	{
		const QE_DataType2 _default = {.rawAngle = {0}, .motorID = -1};
		return _default;
	}

	const QE_DataType2 temp = queue->data[queue->head];
	queue->head = (queue->head + 1) % queue->capacity;
	queue->size--;
	return temp;
}

bool Queue2Empty(const Queue2 *queue)
{
	return queue->size == 0;
}
